package dk.rafaelcouto.PhoneTone_Extractor;

public class Goertzel2
{
    private double Q1;
    private double Q2;
    private double coeff;
    private double cosine;
    private int n;
    private float samplingRate;
    private double sine;
    
    public Goertzel2(final float samplingRate, final int n) {
        this.samplingRate = samplingRate;
        this.n = n;
    }
    
    public double[] getRealImag(final double[] array) {
        array[0] = this.Q1 - this.Q2 * this.cosine;
        array[1] = this.Q2 * this.sine;
        return array;
    }
    
    public void initGoertzel(final float n) {
        this.sine = Math.sin(6.283185307179586 * (n / this.samplingRate));
        this.cosine = Math.cos(6.283185307179586 * (n / this.samplingRate));
        this.coeff = 2.0 * this.cosine;
        final float n2 = this.n;
        final double n3 = 6.283185307179586 * (int)(0.5 + n2 * n / this.samplingRate) / n2;
        this.sine = Math.sin(n3);
        this.cosine = Math.cos(n3);
        this.coeff = 2.0 * this.cosine;
        this.resetGoertzel();
    }
    
    public double processSamples(final short[] array) {
        for (int length = array.length, i = 0; i < length; ++i) {
            final double q1 = this.coeff * this.Q1 - this.Q2 + array[i];
            this.Q2 = this.Q1;
            this.Q1 = q1;
        }
        return this.Q1 * this.Q1 + this.Q2 * this.Q2 - this.Q1 * this.Q2 * this.coeff;
    }
    
    public void resetGoertzel() {
        this.Q1 = 0.0;
        this.Q2 = 0.0;
    }
}
